﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using OpenTK;
using OpenTK.Graphics.OpenGL4;
using OpenTK.Input;
using System.Drawing;
using OpenTK.Graphics;
using Common;
using BulletSharp;
using BulletSharp.Math;
using BulletSharp.SoftBody;
using BVector3 = BulletSharp.Math.Vector3;
using BQuaternion = BulletSharp.Math.Quaternion;


namespace T19_Misc05_3_picking_BulletPhysics {
    using AntTweakBar;
    using OpenTK;
    class T19_Misc05_3_picking_BulletPhysicsGameWindow : GameWindow {
        public T19_Misc05_3_picking_BulletPhysicsGameWindow() {
            this.Load += T19_Misc05_3_picking_BulletPhysicsGameWindow_Load;
            this.Resize += T19_Misc05_3_picking_BulletPhysicsGameWindow_Resize;
            this.RenderFrame += T19_Misc05_3_picking_BulletPhysicsGameWindow_RenderFrame;
            this.Unload += T19_Misc05_3_picking_BulletPhysicsGameWindow_Unload;
            this.Width = 1024;
            this.Height = 768;
            this.Title = "T19_Misc05_3_picking_BulletPhysicsGameWindow";

        }

        void ScreenPosToWorldRay(
            int mouseX, int mouseY,             // Mouse position, in pixels, from bottom-left corner of the window
            int screenWidth, int screenHeight,  // Window size, in pixels
            Matrix4 ViewMatrix,               // Camera position and orientation
            Matrix4 ProjectionMatrix,         // Camera parameters (ratio, field of view, near and far planes)
            out Vector3 out_origin,              // Ouput : Origin of the ray. /!\ Starts at the near plane, so if you want the ray to start at the camera's position instead, ignore this.
            out Vector3 out_direction            // Ouput : Direction, in world space, of the ray that goes "through" the mouse.
                                    ) {

            // The ray Start and End positions, in Normalized Device Coordinates (Have you read Tutorial 4 ?)
            Vector4 lRayStart_NDC = new Vector4(
                ((float)mouseX / (float)screenWidth - 0.5f) * 2.0f, // [0,1024] -> [-1,1]
                ((float)mouseY / (float)screenHeight - 0.5f) * 2.0f, // [0, 768] -> [-1,1]
                -1.0f, // The near plane maps to Z=-1 in Normalized Device Coordinates
               1.0f
               );
            Vector4 lRayEnd_NDC = new Vector4(
                ((float)mouseX / (float)screenWidth - 0.5f) * 2.0f,
                ((float)mouseY / (float)screenHeight - 0.5f) * 2.0f,
                0.0f,
                1.0f
                );


            // The Projection matrix goes from Camera Space to NDC.
            // So inverse(ProjectionMatrix) goes from NDC to Camera Space.
            //Matrix4 InverseProjectionMatrix = glm::inverse(ProjectionMatrix);
            Matrix4 InverseProjectionMatrix = ProjectionMatrix.Inverted();

            // The View Matrix goes from World Space to Camera Space.
            // So inverse(ViewMatrix) goes from Camera Space to World Space.
            Matrix4 InverseViewMatrix = ViewMatrix.Inverted();

            Vector4 lRayStart_camera = lRayStart_NDC * InverseProjectionMatrix; lRayStart_camera /= lRayStart_camera.W;
            Vector4 lRayStart_world = lRayStart_camera * InverseViewMatrix; lRayStart_world /= lRayStart_world.W;
            Vector4 lRayEnd_camera = lRayEnd_NDC * InverseProjectionMatrix; lRayEnd_camera /= lRayEnd_camera.W;
            Vector4 lRayEnd_world = lRayEnd_camera * InverseViewMatrix; lRayEnd_world /= lRayEnd_world.W;


            // Faster way (just one inverse)
            //glm::mat4 M = glm::inverse(ProjectionMatrix * ViewMatrix);
            //glm::vec4 lRayStart_world = M * lRayStart_NDC; lRayStart_world/=lRayStart_world.w;
            //glm::vec4 lRayEnd_world   = M * lRayEnd_NDC  ; lRayEnd_world  /=lRayEnd_world.w;


            Vector3 lRayDir_world = new Vector3(lRayEnd_world - lRayStart_world);
            //lRayDir_world = glm::normalize(lRayDir_world);
            lRayDir_world = lRayDir_world.Normalized();


            out_origin = new Vector3(lRayStart_world);
            //out_direction = glm::normalize(lRayDir_world);
            out_direction = lRayDir_world.Normalized();
        }

        private void T19_Misc05_3_picking_BulletPhysicsGameWindow_RenderFrame(object sender, FrameEventArgs e) {
            GL.ClearColor(.1f, .1f, .99f, .9f);
            GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

            Render();

            context.Draw();
            SwapBuffers();
            ProcessEvents();
        }

        const int objectCount = 2;
        Vector3[] positions = new Vector3[objectCount];
        Quaternion[] orientations = new Quaternion[objectCount];

        int rand() {
            return random.Next();
        }
        Random random = new Random();

        private void InitializePosOrient() {
            for (int i = 0; i < positions.Length; i++) {
                positions[i] = new Vector3(rand() % 20 - 10, rand() % 20 - 10, rand() % 20 - 10);
                orientations[i] = new Quaternion(rand() % 360, rand() % 360, rand() % 360);
            }
        }

        Context context;
        StringVariable messageVar;

        private void InitializeBars() {
            context = new Context(Tw.GraphicsAPI.OpenGL);
            Bar GUI = new Bar(context) { Label = "Picking", ValueColumnWidth = 100, Size = new Size(340, 200) };
            GUI.SetDefinition("refresh=0.1");
            messageVar = new StringVariable(GUI) { Label = "Last picked object", };
            messageVar.Value = "test";
        }
        protected override void OnResize(EventArgs e) {
            base.OnResize(e);
            context.HandleResize(this.ClientSize);
        }

        Vao vao;
        Shader shader;
        ModelMesh mesh;
        Texture2D texture;
        VertexAttribManager attribs = new VertexAttribManager();

        BroadphaseInterface broadphase;
        DefaultCollisionConfiguration collisionConfiguration;
        CollisionDispatcher dispatcher;
        SequentialImpulseConstraintSolver solver;
        DiscreteDynamicsWorld dynamicsWorld;
        List<RigidBody> rigidbodies = new List<RigidBody>();
        CollisionShape boxCollisionShape;
        private void T19_Misc05_3_picking_BulletPhysicsGameWindow_Load(object sender, EventArgs e) {
            InitializeBars();
            VSync = VSyncMode.Off;
            GL.Enable(EnableCap.DepthTest);
            GL.DepthFunc(DepthFunction.Less);
            GL.Enable(EnableCap.CullFace);

            vao = new Vao();
            shader = new Shader("shader.vs", "shader.fs");

            mesh = MeshLoader.LoadObjIndexed("suzanne.obj");
            mesh.CreateVbo();

            texture = Texture2D.Load("uvmap.dds");
            InitializePosOrient();

            attribs.AddAttrib(3, mesh.VertexBuffer);
            attribs.AddAttrib(2, mesh.UvBuffer);
            attribs.AddAttrib(3, mesh.NormalBuffer);

            IniBullet();


        }

        private void IniBullet() {
             broadphase = new DbvtBroadphase();

            // Set up the collision configuration and dispatcher
            collisionConfiguration = new DefaultCollisionConfiguration();
            dispatcher = new CollisionDispatcher(collisionConfiguration);

            // The actual physics solver
            solver = new SequentialImpulseConstraintSolver();

            // The world.
            dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
            var gravity = new BVector3(0f, -9.81f, 0f);
            dynamicsWorld.SetGravity(ref gravity);





            //std::vector<btRigidBody*> rigidbodies;

            // In this example, all monkeys will use the same collision shape : 
            // A box of 2m*2m*2m (1.0 is the half-extent !)
            boxCollisionShape = new BoxShape(new BVector3(1.0f, 1.0f, 1.0f));
            

            for (int i = 0; i < positions.Length; i++) {

                BQuaternion rotation= new BQuaternion(orientations[i].X, orientations[i].Y, orientations[i].Z, orientations[i].W);
                BVector3 translation = new BVector3(positions[i].X, positions[i].Y, positions[i].Z);
                Matrix mTranslation = Matrix.Translation(translation);
                Matrix mRotation = Matrix.RotationQuaternion(rotation);
                //Matrix transform = mTranslation * mRotation;
                Matrix transform = mRotation *mTranslation ;

                DefaultMotionState motionstate = new DefaultMotionState(transform);

                //btRigidBody::bt
                    RigidBodyConstructionInfo rigidBodyCI=new RigidBodyConstructionInfo(
        
            0,                  // mass, in kg. 0 -> Static object, will never move.
			motionstate,
			boxCollisionShape,  // collision shape of body
			new BVector3(0, 0, 0)    // local inertia
		);
            RigidBody rigidBody = new RigidBody(rigidBodyCI);

            rigidbodies.Add(rigidBody);
                dynamicsWorld.AddRigidBody(rigidBody);

            // Small hack : store the mesh's index "i" in Bullet's User Pointer.
            // Will be used to know which object is picked. 
            // A real program would probably pass a "MyGameObjectPointer" instead.
            //rigidBody.setUserPointer((void*)i);
                rigidBody.UserIndex = i;

        }
    }

        private void Render() {
            
            InputControls.computeMatricesFromInput(this);
            dynamicsWorld.StepSimulation((float)InputControls.getDeltaTime(),7);
            Matrix4 ProjectionMatrix = InputControls.ProjectionMatrix;
            Matrix4 ViewMatrix = InputControls.ViewMatrix;




            // PICKING IS DONE HERE
            // (Instead of picking each frame if the mouse button is down, 
            // you should probably only check if the mouse button was just released)
            if (InputControls.isMouseButtonDown(MouseButton.Left)) {


                Vector3 out_origin;
                Vector3 out_direction;
                ScreenPosToWorldRay(
                    1024 / 2, 768 / 2,
                    1024, 768,
                    ViewMatrix,
                    ProjectionMatrix,
                    out out_origin,
                    out out_direction
                );

                Vector3 out_end = out_origin + out_direction * 1000.0f;

                //btCollisionWorld::ClosestRayResultCallback RayCallback(btVector3(out_origin.x, out_origin.y, out_origin.z), btVector3(out_end.x, out_end.y, out_end.z));
                BVector3 bout_origin = new BVector3(out_origin.X, out_origin.Y, out_origin.Z);
                BVector3 bout_end = new BVector3(out_end.X, out_end.Y, out_end.Z);
                ClosestRayResultCallback RayCallback = new ClosestRayResultCallback(ref bout_origin,ref bout_end);
                //dynamicsWorld->rayTest(btVector3(out_origin.x, out_origin.y, out_origin.z), btVector3(out_end.x, out_end.y, out_end.z), RayCallback);
                dynamicsWorld.RayTest(new BVector3(out_origin.X, out_origin.Y, out_origin.Z),  new BVector3(out_end.X, out_end.Y, out_end.Z), RayCallback);
                if (RayCallback.HasHit) {
                    //std::ostringstream oss;
                    //oss << "mesh " << (size_t)RayCallback.m_collisionObject->getUserPointer();
                    //message = oss.str();
                    messageVar.Value = $"mesh {RayCallback.CollisionObject.UserIndex}";
                }
                else {
                    messageVar.Value = "background";
                }


            }


            // Dark blue background
            GL.ClearColor(0.0f, 0.0f, 0.4f, 0.0f);
            // Re-clear the screen for real rendering
            GL.Clear( ClearBufferMask.ColorBufferBit |  ClearBufferMask.DepthBufferBit);


            // Use our shader
            shader.Use();

            //glEnableVertexAttribArray(0);
            //glEnableVertexAttribArray(1);
            //glEnableVertexAttribArray(2);

            for (int i = 0; i < positions.Length; i++) {


                Matrix4 RotationMatrix = Matrix4.CreateFromQuaternion(orientations[i]);
                Matrix4 TranslationMatrix = Matrix4.CreateTranslation(positions[i]);
                //glm::mat4 ModelMatrix = TranslationMatrix * RotationMatrix;
                Matrix4 ModelMatrix = RotationMatrix *TranslationMatrix ;

                Matrix4 MVP = ModelMatrix * ViewMatrix * ProjectionMatrix;

                // Send our transformation to the currently bound shader, 
                // in the "MVP" uniform
                //glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &MVP[0][0]);
                //glUniformMatrix4fv(ModelMatrixID, 1, GL_FALSE, &ModelMatrix[0][0]);
                //glUniformMatrix4fv(ViewMatrixID, 1, GL_FALSE, &ViewMatrix[0][0]);
                shader.SetMatrix4("MVP", ref MVP);
                shader.SetMatrix4("M", ref ModelMatrix);
                shader.SetMatrix4("V", ref ViewMatrix);

                Vector3 lightPos = new Vector3(4, 4, 4);
                //glUniform3f(LightID, lightPos.x, lightPos.y, lightPos.z);
                shader.SetVector3("LightPosition_worldspace", lightPos);

                //// Bind our texture in Texture Unit 0
                //glActiveTexture(GL_TEXTURE0);
                //glBindTexture(GL_TEXTURE_2D, Texture);
                //// Set our "myTextureSampler" sampler to user Texture Unit 0
                //glUniform1i(TextureID, 0);
                texture.Active(TextureUnit.Texture0, shader, "myTextureSampler");

                // 1rst attribute buffer : vertices
                //glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer);
                //glVertexAttribPointer(
                //    0,                  // attribute
                //    3,                  // size
                //    GL_FLOAT,           // type
                //    GL_FALSE,           // normalized?
                //    0,                  // stride
                //    (void*)0            // array buffer offset
                //);

                //// 2nd attribute buffer : UVs
                //glBindBuffer(GL_ARRAY_BUFFER, uvbuffer);
                //glVertexAttribPointer(
                //    1,                                // attribute
                //    2,                                // size
                //    GL_FLOAT,                         // type
                //    GL_FALSE,                         // normalized?
                //    0,                                // stride
                //    (void*)0                          // array buffer offset
                //);

                //// 3rd attribute buffer : normals
                //glBindBuffer(GL_ARRAY_BUFFER, normalbuffer);
                //glVertexAttribPointer(
                //    2,                                // attribute
                //    3,                                // size
                //    GL_FLOAT,                         // type
                //    GL_FALSE,                         // normalized?
                //    0,                                // stride
                //    (void*)0                          // array buffer offset
                //);
                attribs.Enable();
                // Index buffer
                //glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, elementbuffer);
                mesh.IndexBuffer.Bind();
                // Draw the triangles !
                //glDrawElements(
                //    GL_TRIANGLES,      // mode
                //    indices.size(),    // count
                //    GL_UNSIGNED_SHORT,   // type
                //    (void*)0           // element array buffer offset
                //);
                GL.DrawElements(BeginMode.Triangles, mesh.Indices.Length, DrawElementsType.UnsignedShort, 0);
                attribs.Disable();

            }

            //glDisableVertexAttribArray(0);
            //glDisableVertexAttribArray(1);
            //glDisableVertexAttribArray(2);

        }

        private void T19_Misc05_3_picking_BulletPhysicsGameWindow_Resize(object sender, EventArgs e) {
            GL.Viewport(0, 0, Width, Height);
        }



        private void T19_Misc05_3_picking_BulletPhysicsGameWindow_Unload(object sender, EventArgs e) {
            GlDisposableObjectBase.DisposeAllDisposableMembers(this);
        }

    }
}
